#ifndef astarclass
#define astarclass

#include <allegro.h>
#include "game_astar.h"

AstarClass::AstarClass( int  *map, int width, int height, int tilesize )
{
    Stack  =  ( STACK * )calloc( 1 , sizeof( STACK ) );
    isPath  =  false;
    OpenNode  =  NULL;
    ClosedNode  =  NULL;
    PathNode  =  NULL;
	AllowDiagonalMovement=true;

	//lines below used to be a method called InitAstarTileMap( map );
	//but pointless as never called really outside of constructor
    Width  =  width;
    Height  =  height;
    TotalSize  =  Height *Width;
	TileSize=tilesize;

	int i,j;
    TileMap  =  new int[ TotalSize ];
    for ( i = 0; i < Width; i ++  )
    {
        for ( j = 0; j < Height; j ++  )
        {
            TileMap[ i + j * Width ]  =  map[ i + j * Width ];
        }
    }

	First=NULL;
}

AstarClass::~AstarClass()
{
    FreeNodes();
    free( Stack );
    if ( TileMap )
        delete []TileMap;
}

// If we did any changes to the map then we would have to fix the astar class  
void AstarClass::RedoAstarTileMap( int  *map )
{
    for ( int i = 0; i < Width; i ++  )
    {
        for ( int j = 0; j < Height; j ++  )
        {
            TileMap[ i + j * Width ]  =  map[ i + j * Width ];
        }
    }
}

//find a new path using source and destination
//can be time consuming so maybe link to a timer or know only when changed locations
bool AstarClass::NewPath( int source_x , int source_y , int dest_x , int dest_y, bool dm )
{
	this->AllowDiagonalMovement=dm;
    if ( FreeTile( dest_x , dest_y ) && FreeTile( source_x , source_y ) && 
         ( TileNum( source_x , source_y ) != TileNum( dest_x , dest_y ) ) )
    {
        isPath = true;
        FreeNodes();
        FindPath( source_x , source_y , dest_x , dest_y );        
        return ( isPath );
    }

	First=NULL;
    return ( isPath = false );
}

//have we reached our goal yet
//returns true if 
//	a) there is no path
//	b) there is a path and no more items in list
bool AstarClass::ReachedGoal()
{
	//is there an open path to the destination
    if ( !isPath ) return true;
    if ( PathNode->Parent  !=  NULL ) return false;

    return true;
}

//get the tile bit using a pixel location
int AstarClass::TileNum( int x , int y )
{
    return ( ( x / TileSize ) + ( y / TileSize * Width ) );
}

//is there an empty tile at block location x/y
bool AstarClass::FreeTile( int x , int y )
{
	if(x<0 || y<0 || x >= (Width*TileSize)) return false;	//can't go off edge (bottom already ok)
    if ( TileMap[ ( x / TileSize ) + ( y / TileSize ) * Width]  ==  0 ) return true;
    
    return false;
}

void AstarClass::PathNextNode()
{
    PathNode = PathNode->Parent;
	if(!First) First=PathNode;
}

int AstarClass::NodeGetX()
{
    return PathNode->x;
}

int AstarClass::NodeGetY()
{
    return PathNode->y;
}

void AstarClass::NodeGoFirst()
{
	PathNode=First;
}

/* private stuff */
void AstarClass::FreeNodes()
{
    NODE  *NODE ,  *OldNODE;

    if ( OpenNode  !=  NULL )
    {
        NODE  =  OpenNode->NextNode;

        while ( NODE  !=  NULL )
        {
            OldNODE  =  NODE;
            NODE  =  NODE->NextNode;
            free( OldNODE );
        }
    }

    if ( ClosedNode  !=  NULL )
    {
        NODE  =  ClosedNode->NextNode;

        while ( NODE  !=  NULL )
        {
            OldNODE  =  NODE;
            NODE  =  NODE->NextNode;
            free( OldNODE );
        }
    }
}

void AstarClass::FindPath( int source_x , int source_y , int dest_x , int dest_y )
{
    NODE  *Node ,  *BestNode;
    int TileNumDest;

    isPath = true;
    TileNumDest  =  TileNum( source_x , source_y );

    OpenNode = ( NODE * )calloc( 1 , sizeof( NODE ) );
    ClosedNode = ( NODE * )calloc( 1 , sizeof( NODE ) );

    Node = ( NODE * )calloc( 1 , sizeof( NODE ) );
    Node->g  =  0;
    Node->h  =  distance( ( dest_x - source_x ) , ( dest_y - source_y ) );
    Node->f  =  Node->g + Node->h;
    Node->NodeNum  =  TileNum( dest_x , dest_y );
    Node->x  =  dest_x;
    Node->y  =  dest_y;

    OpenNode->NextNode = Node;
    for ( ;; )
    {
        BestNode = ReturnBestNode();

        if ( BestNode == NULL ) break;

        if ( BestNode->NodeNum  ==  TileNumDest )
            break;

        GenerateSuccessors( BestNode , source_x , source_y );
    }

    PathNode  =  BestNode;
	First=PathNode;
}

AstarClass::NODE  *AstarClass::ReturnBestNode()
{
    NODE  *tmp;

    if ( OpenNode->NextNode  ==  NULL )
    {
        isPath = false;
        tmp = NULL;
        return tmp;
    }

    tmp  =  OpenNode->NextNode;
    OpenNode->NextNode  =  tmp->NextNode;
    tmp->NextNode  =  ClosedNode->NextNode;
    ClosedNode->NextNode  =  tmp;

   return( tmp );
}
void AstarClass::GenerateSuccessors( NODE  *BestNode , int dest_x , int dest_y )
{
    int x , y;

// Upper - Left  
    if ( AllowDiagonalMovement && FreeTile( x = BestNode->x - TileSize , y = BestNode->y - TileSize ) )
        GenerateSucc( BestNode , x , y , dest_x , dest_y );

// Upper  
    if ( FreeTile( x = BestNode->x , y = BestNode->y - TileSize ) )
        GenerateSucc( BestNode , x , y , dest_x , dest_y );

// Upper - Right  
    if ( AllowDiagonalMovement && FreeTile( x = BestNode->x + TileSize , y = BestNode->y - TileSize ) )
        GenerateSucc( BestNode , x , y , dest_x , dest_y );

//  Right
    if ( FreeTile( x = BestNode->x + TileSize , y = BestNode->y ) )
        GenerateSucc( BestNode , x , y , dest_x , dest_y );

// Lower - Right  
    if ( AllowDiagonalMovement && FreeTile( x = BestNode->x + TileSize , y = BestNode->y + TileSize ) )
        GenerateSucc( BestNode , x , y , dest_x , dest_y );

// Lower  
    if ( FreeTile( x = BestNode->x , y = BestNode->y + TileSize ) )
        GenerateSucc( BestNode , x , y , dest_x , dest_y );

// Lower - Left  
    if ( AllowDiagonalMovement && FreeTile( x = BestNode->x - TileSize , y = BestNode->y + TileSize ) )
        GenerateSucc( BestNode , x , y , dest_x , dest_y );

// Left  
    if ( FreeTile( x = BestNode->x - TileSize , y = BestNode->y ) )
        GenerateSucc( BestNode , x , y , dest_x , dest_y );
}

void AstarClass::GenerateSucc( NODE  *BestNode , int x , int y , int dest_x , int dest_y )
{
    int g , TileNumS , c  =  0;
    NODE  *Old ,  *Successor;
    
    g  =  BestNode->g + 1;
    TileNumS  =  TileNum( x , y );

    if ( ( Old = CheckOPEN( TileNumS ) )  !=  NULL ) 
    {
        for( c  =  0; c  <  MAX_CHILDREN; c ++  )
        {
            if( BestNode->Child[ c ]  ==  NULL )            
            {
                break;
            }
        }
        BestNode->Child[ c ]  =  Old;

        if ( g  <  Old->g )
        {
            Old->Parent  =  BestNode;
            Old->g  =  g;
            Old->f  =  g + Old->h;
        }
    }
    else if ( ( Old = CheckCLOSED( TileNumS ) )  !=  NULL ) 
    {
        for( c  =  0; c <  MAX_CHILDREN; c ++  )
        {
            if ( BestNode->Child[ c ]  ==  NULL )
                break;
        }
        
        BestNode->Child[ c ]  =  Old;

        if ( g  <  Old->g )
        {
            Old->Parent  =  BestNode;
            Old->g  =  g;
            Old->f  =  g + Old->h;
            PropagateDown( Old );  
        }
    }
    else
    {
        Successor  =  ( NODE* )calloc( 1 , sizeof( NODE ) );

        Successor->Parent  =  BestNode;
        Successor->g  =  g;
        Successor->h  =  distance( ( x - dest_x ) , ( y - dest_y ) );
        Successor->f  =  g + Successor->h;
        Successor->x  =  x;
        Successor->y  =  y;
        Successor->NodeNum  =  TileNumS;
        Insert( Successor );    
        for( c  = 0; c  <  MAX_CHILDREN; c ++  )
        {
            if ( BestNode->Child[ c ]  ==  NULL )
                break;
        }
        BestNode->Child[ c ]  =  Successor;
    }
}

AstarClass::NODE  *AstarClass::CheckOPEN( int tilenum )
{
    NODE  *tmp;

    tmp  =  OpenNode->NextNode;
    while ( tmp  !=  NULL )
    {
        if ( tmp->NodeNum  ==  tilenum )
        {
            return ( tmp );
        }
        else
        {
            tmp  =  tmp->NextNode;
        }
    }

    return( NULL );
}

AstarClass::NODE * AstarClass::CheckCLOSED( int tilenum )
{
    NODE  *tmp;

    tmp  =  ClosedNode->NextNode;

    while ( tmp  !=  NULL )
    {
        if ( tmp->NodeNum  ==  tilenum )
        {
            return( tmp );
        }
        else
        {
            tmp  =  tmp->NextNode;
        }
    }

    return( NULL );
}

void AstarClass::Insert( NODE  *Successor )
{
    NODE  *tmp1 ,  *tmp2;
    int f;

    if ( OpenNode->NextNode  ==  NULL )
    {
        OpenNode->NextNode  =  Successor;
        return;
    }

     //  insert into OpenNode successor wrt f
    f  =  Successor->f;
    tmp1  =  OpenNode;
    tmp2  =  OpenNode->NextNode;

    while ( ( tmp2  !=  NULL ) && ( tmp2->f  <  f ) )
    {
        tmp1  =  tmp2;
        tmp2  =  tmp2->NextNode;
    }

    Successor->NextNode  =  tmp2;
    tmp1->NextNode  =  Successor;
}

void AstarClass::PropagateDown( NODE  *Old )
{
    int c , g;
    NODE  *Child ,  *Father;

    g  =  Old->g;

    for ( c  =  0; c  <  MAX_CHILDREN; c ++  )
    {
        if ( ( Child = Old->Child[ c ] )  ==  NULL )
            break;
        
        if ( g + 1  <  Child->g )
        {
            Child->g  =  g + 1;
            Child->f  =  Child->g + Child->h;
            Child->Parent  =  Old;

            Push( Child );         
        }     
    }

    while ( Stack->NextStackPtr  !=  NULL )
    {
        Father  =  Pop();
        for ( c  =  0; c < MAX_CHILDREN; c ++  )
        {
            if ( ( Child = Father->Child[ c ] )  ==  NULL )
                break;

            if ( Father->g + 1  <  Child->g )
            {              
                Child->g  =  Father->g + 1;
                Child->f  =  Child->g + Child->h;
                Child->Parent  =  Father;
                Push( Child );
            }
        }
    }
}

void AstarClass::Push( NODE  *NODE )
{
    STACK  *tmp;

    tmp  = ( STACK * )calloc( 1 , sizeof( STACK ) );
    tmp->NodePtr  =  NODE;
    tmp->NextStackPtr  =  Stack->NextStackPtr;
    Stack->NextStackPtr  =  tmp;
}

AstarClass::NODE  *AstarClass::Pop()
{
    NODE  *tmp;
    STACK  *tmpSTK;

    tmpSTK  =  Stack->NextStackPtr;
    tmp  =  tmpSTK->NodePtr;

    Stack->NextStackPtr  =  tmpSTK->NextStackPtr;
    free( tmpSTK );

   return( tmp );
}

int AstarClass::distance( int x , int y )
{
	//return abs(abs(x)+abs(y));
    return fixtoi( fhypot( itofix( x ) , itofix( y ) ) );
}


/*
//  Global Variables   
BITMAP      *buffer;
BITMAP      *sprite;
AstarClass  *astar;
int         posX;
int         posY;
int         tilemap[ MAZE_WIDTH * MAZE_HEIGHT ]  = 
{   
    1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
    1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,1,
    1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,0,1,0,1,1,1,1,1,0,1,1,1,0,1,1,0,1,1,
    1,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0,0,0,1,0,1,0,0,0,0,1,0,1,1,
    1,0,1,0,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,0,1,0,1,0,1,1,1,1,1,1,0,1,1,
    1,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0,1,0,0,0,0,0,0,0,0,1,1,
    1,0,1,1,1,0,1,1,1,1,1,0,1,0,1,0,1,0,1,1,1,1,1,1,1,1,1,0,1,0,1,0,1,1,1,1,1,1,1,1,1,
    1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1,0,0,0,0,0,0,1,1,1,1,
    1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,0,1,0,1,1,
    1,0,1,0,0,0,0,0,1,0,0,0,0,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,1,0,1,1,
    1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,0,1,0,1,1,
    1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,
    1,1,1,1,1,1,1,0,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,1,1,
    1,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0,1,1,
    1,0,1,1,1,0,1,1,1,1,1,1,1,0,1,0,1,1,1,1,1,0,1,0,1,0,1,1,1,1,1,0,1,1,1,1,0,1,0,1,1,
    1,0,1,0,1,0,1,0,0,0,0,0,1,0,1,0,1,0,0,0,0,0,1,0,1,0,0,0,0,0,1,0,1,0,0,0,0,1,0,1,1,
    1,1,1,0,1,0,1,1,1,0,1,1,1,0,1,0,1,1,1,1,1,1,1,0,1,1,1,0,1,1,1,0,1,1,1,1,0,1,0,1,1,
    1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,1,0,0,0,0,0,0,1,1,
    1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,
    1,0,1,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,1,0,0,0,1,0,1,0,0,0,0,0,0,1,1,
    1,0,1,0,1,1,1,1,1,0,1,0,1,0,1,1,1,1,1,1,1,1,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,1,0,1,1,
    1,0,1,0,1,0,0,0,1,0,1,0,1,0,1,0,1,0,0,0,0,0,1,0,1,0,1,0,1,0,1,0,1,0,0,0,0,1,0,1,1,
    1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,1,0,1,1,1,1,
    1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,0,0,0,0,0,0,0,1,1,
    1,0,1,0,1,0,1,0,1,0,1,0,0,0,1,0,1,0,1,0,0,0,0,0,1,0,1,0,1,0,1,0,1,1,1,1,1,1,0,1,1,
    1,0,1,0,1,0,1,1,1,0,1,1,0,1,1,0,1,0,1,1,1,1,1,1,1,0,1,0,1,1,1,0,1,0,0,0,0,1,0,1,1,
    1,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0,1,0,1,0,0,1,0,1,1,
    1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,1,0,1,1,1,1,0,1,1,
    1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,
    1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
    1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1 
};
 
BITMAP *create_ball();
void deleteAll();
void drawAll();
void drawMouse( BITMAP  *bmp );
int initAll();
int random( int u );
int main();


int random( int u )
{
    return ( rand() % u );
}

void drawMouse( BITMAP  *bmp )
{
    triangle( bmp , mouse_x , mouse_y , mouse_x - 5 , mouse_y + 15 , mouse_x + 5 , mouse_y + 15 , makecol( 0 , 80 , 0 ) );
    triangle( bmp , mouse_x , mouse_y + 1 , mouse_x - 4 , mouse_y + 14 , mouse_x + 4 , mouse_y + 14 , makecol( 0 , 255 , 0 ) );
}

void drawAll()
{
    int color[ 2 ];

    color[ 0 ] = makecol( 0 , 0 , 0 );
    color[ 1 ] = makecol( 255 , 0 , 0 );

    clear_bitmap( buffer );

    for ( int i  =  0; i  <  MAZE_WIDTH; i ++  )
    {
        for ( int j  =  0; j  <  MAZE_HEIGHT; j ++  )
        {
            rectfill( buffer , i * TileSize , j * TileSize , ( i + 1 ) * TileSize - 1 , ( j + 1 ) * TileSize - 1 , color[ tilemap[ i + j * MAZE_WIDTH ] ] );
        }
    }
    draw_sprite( buffer , sprite , posX - TileSize / 2 , posY - TileSize / 2 );
    drawMouse( buffer );

    acquire_screen();
    blit( buffer , screen , 0 , 0 , 0 , 0 , SCREEN_W , SCREEN_H );
    release_screen();
}

BITMAP *create_ball()
{
    BITMAP * temp = create_bitmap( TileSize , TileSize );
    clear_to_color( temp , makecol( 255 , 0 , 255 ) );
    circlefill( temp , temp->w / 2 , temp->h / 2 , TileSize / 3 , makecol( 0 , 255 , 0 ) );

    return temp;
}

int initAll()
{
    allegro_init();
    install_keyboard();
    install_timer();
    install_mouse();

    set_color_depth( 16 );

    if ( set_gfx_mode( GFX_AUTODETECT , 640 , 480 , 0 , 0 ) < 0 )
    {
        allegro_message( "Unable initialize graphics module\n%s\n" , allegro_error );
        return  - 1;
    }

    srand( time( NULL ) );
    buffer = create_bitmap( SCREEN_W , SCREEN_H );
    clear( buffer );
    sprite = create_ball();
    astar  =  new AstarClass( tilemap );

    posX = posY = TileSize + TileSize / 2;

    return 0;
}

void deleteAll()
{
    if ( astar )
        delete astar;
    if ( buffer )
        destroy_bitmap( buffer );
    if ( sprite )
        destroy_bitmap( sprite );
}

int main()
{
    bool done = false;
    int destX , destY , mx , my , ky = 0;

    if ( initAll() < 0 ) return  - 1;

    destX = posX;
    destY = posY;

    while ( !done )
    {
        drawAll();
        if ( key[ KEY_ESC ] ) done = true;

        if ( mouse_b & 1 )
        {
            mx = mouse_x - ( mouse_x % TileSize ) + TileSize / 2;
            my = mouse_y - ( mouse_y % TileSize ) + TileSize / 2;

            if ( mx < ( MAZE_WIDTH * TileSize ) && my < ( MAZE_HEIGHT * TileSize ) )
            {
                if ( astar->NewPath( posX , posY , mx , my ) )
                {
                    astar->PathNextNode();
                    destX  =  astar->NodeGetX();
                    destY  =  astar->NodeGetY();
                }
            }
        }

        if ( !astar->ReachedGoal() && ( destX == posX ) && ( destY == posY ) )
        {
            astar->PathNextNode();
            destX  =  astar->NodeGetX();
            destY  =  astar->NodeGetY();
        }

        for ( int i = 0; i < ( TileSize / 4 ); i ++  )
        {
            if ( destX  >  posX ) posX ++ ;
            if ( tilemap[ ( posX / TileSize )  + ( ( posY / TileSize ) * MAZE_WIDTH ) ] == 1 ) posX -- ;

            if ( destY  >  posY ) posY ++ ;
            if ( tilemap[ ( posX / TileSize )  + ( ( posY / TileSize ) * MAZE_WIDTH ) ] == 1 ) posY -- ;

            if ( destX  <  posX ) posX -- ;
            if ( tilemap[ ( posX / TileSize )  + ( ( posY / TileSize ) * MAZE_WIDTH ) ] == 1 ) posX ++ ;

            if ( destY  <  posY ) posY -- ;
            if ( tilemap[ ( posX / TileSize )  + ( ( posY / TileSize ) * MAZE_WIDTH ) ] == 1 ) posY ++ ;
        }
    }

    deleteAll();
    return 0;
}
END_OF_MAIN();

*/
#endif
